碰撞阈值自动计算流程

[TOC]

1.碰撞阈值激励轨迹生成

/**
    * @brief 生成碰撞阈值激励轨迹
    * @param robot: 机器人指针
    * @param sample_period: 规划周期，一般采用5ms
    * @param max_joint_vel: 最大速度限制
    * @param max_joint_acc: 最大加速度限制
    * @param collision_traj: 碰撞阈值激励轨迹
    * @return 返回值< 0 表示计算失败
    */
int FPCalculateCollisionThresholdExcitationTrajectory(const ARAL::interface::ARALIntfacePtr& robot, const double& sample_period, const interface::RLJntArray& max_joint_vel, const interface::RLJntArray& max_joint_acc, std::vector<interface::RLJntArray>& collision_traj);

调用该接口时，max_joint_vel与max_joint_acc分别设置为对应臂形config文件中的最大速度和加速度（v_joint_override与a_joint_override）

2.碰撞数据采集

1）机械臂运行采用上述接口生成的碰撞阈值激励轨迹，分别采集空载数据A和满载数据B；

2）诊断数据中需包括关节角，关节速度，关节加速度，温度，电流，摩擦力矩等数据；

3.碰撞阈值表自动计算

3.1自动计算阈值功能包接口介绍

/**
    * @brief 生成碰撞阈值表格
    * @param robot: 机器人指针
    * @param csv_data: 读取数据结构体，具体含义参见CollisionThresholdParams
    * @param collision_threshold_table: 阈值表
    * @return 返回值< 0 表示生成阈值表失败
    */
int FPCalCollisionThresholdTable(const ARAL::interface::ARALIntfacePtr& robot, const CollisionThresholdParams& csv_data, std::vector<interface::RLJntArray>& collision_threshold_table);

3.2 使用方法

1）创建算法库实例

    /**
     * @brief 根据资源文件初始化算法库, 设置机械臂模型及配置参数信息(算法会自动加载对应的文件)
     * @param robot_name: 机械臂名称
     * @param path: urdf, meshes等资源文件夹所在的路径(如./aral_export/aubo_description/)
     * @return if < 0, 表示初始化失败
     */
    ARAL_API_BASIC(1.0) int rlInitiateRobotModelFromFiles(const std::string& robot_name, const std::string& path) = 0;

2）写入结构体参数

将一组空载数据诊断文件，一组满载数据诊断文件依次写入对应参数中

    interface::Array3d gravity_acc;                                    // 重力加速度
    std::vector<double> dynamic_para;                                 // 动力学参数
    interface::RLJntArray joint_constant;                             // 电流力矩常数
    std::vector<double> Fs;                                           // 摩擦力参数Fs
    std::vector<double> Fc;                                           // 摩擦力参数Fc
    std::vector<double> Vs;                                           // 摩擦力参数Vs
    std::vector<double> Miu;                                          // 摩擦力参数Miu
    std::vector<double> Fv0;                                          // 摩擦力参数Fv0
    std::vector<double> Fv1;                                          // 摩擦力参数Fv1
    std::vector<double> Fv2;                                          // 摩擦力参数Fv2
    std::vector<double> Fv3;                                          // 摩擦力参数Fv3
    std::vector<double> Ft1;                                          // 摩擦力参数Ft1
    std::vector<double> Ft2;                                          // 摩擦力参数Ft2
    std::vector<double> Ft3;                                          // 摩擦力参数Ft3
    std::vector<double> c1;                                           // 摩擦力参数c1
    std::vector<double> c2;                                           // 摩擦力参数c2
    std::vector<interface::RLInertia> tool;                           // 2个诊断文件中的负载信息(顺序为空载,满载)
    std::vector<std::vector<interface::RLJntArray>> pos;              // 2个诊断文件中的关节角(顺序为空载,满载)
    std::vector<std::vector<interface::RLJntArray>> vel;              // 2个诊断文件中的关节速度(顺序同上)
    std::vector<std::vector<interface::RLJntArray>> acc;              // 2个诊断文件中的关节加速度(顺序同上)
    std::vector<std::vector<interface::RLJntArray>> real_cur;         // 2个诊断文件中的实际电流(顺序同上)
    std::vector<std::vector<interface::RLJntArray>> temp;             // 2个诊断文件中的温度(顺序同上)
    std::vector<std::vector<interface::RLJntArray>> friction_tor;     // 2个诊断文件中的摩擦力矩(顺序同上)

3）调用上述FPCalCollisionThresholdTable接口输出碰撞阈值表

4.碰撞阈值表更新与维护

1）计算出阈值表后，形成exl表格交由算法部钟一辰；

2）由软件部姬鹏负责写入aubo_description对应臂形的config文件中，合并MR时由算法部钟一辰review